/*****************************************************
  Mars explorer NXT code in Ch
  University of Leicester GReP 2011
  Using http://iel.ucdavis.edu/projects/chmindstorms/
 *****************************************************/
/*****************************************************
  rtc.ch 
  
  Allows the robot to be controlled manually for
  testing.
 *****************************************************/

#include <conio.h>
#include <stdio.h>
#include <iostream.h>

#include "control.h"

int main()
{   
  int speed=25;	//the speed that the motors will rotate at. (default to 25)

  setup();

	// GUI display
	cout <<   "Movement:    Other Commands:";
	cout << "\n      [w]       [x] Stop Motors";
	cout << "\n   [a][s][d]    [q] Exit Program";
	cout << "\n                [1] Set Speed to 25";
	cout << "\n                [2] Set Speed to 50";
	cout << "\n                [3] Set Speed to 75";
	cout << "\n                [4] Set Speed to 100" << endl;
	cout << "Please Enter command:";
	
	// Control loop. Interprets user command and does action
  key = 'x';
	while (1) 
  {
		checkKey;
    
		switch(key) // is movemode required? test removing it
    {
			case 'w': // forward
				moveCont(speed);
				break;
			case 's': // backward
				moveCont(-speed);
				break;
			case 'd': // right
				rotateCont(speed);
				break;
			case 'a': // left
				rotateCont(-speed);
				break;
			case 'x': // stop
				moveStop();
				break;
			case '1':
				speed = 25;
				break;
			case '2':
				speed = 50;
				break;
			case '3':
				speed = 75;
				break;
			case '4':
				speed = 100;
				break;
			default:
				cout << "\nInvalid Input!" << endl;
		}
	}
  
	disconnect();  
  
  return 0;
}
